#include <QApplication>
#include <QObject>

#include <QGraphicsScene>
#include <QGraphicsView>
#include <QRect>

#include <sys/time.h>

#include "server.h"

//QReadWriteLock m_mutexCamera;

Server::Server()
{
    m_hCOM = 0;
}

Server::~Server()
{
    stop();
}

void Server::initialize()
{
    initThreadVideo();
    initThreadMotor();
    initThreadLaser();
}

void Server::run()
{
    runThreadVideo();
    runThreadMotor();
    runThreadLaser();

    displayData();
}

void Server::stop()
{
    if ( m_hCOM != 0 )
    {
	m_controller.StopMotorRS23233f( m_hCOM );
	m_controller.closeRS232( m_hCOM );
    }
}

void Server::initThreadVideo()
{
    // Thread to get video streaming via http
    m_mjpegClient = new MJPEGClient();
    m_cameraViewer = new ImageViewer( 320, 240 );

    QObject::connect( this->m_mjpegClient, SIGNAL( gotNewImage( const QImage * ) ), this->m_cameraViewer, SLOT( refresh( const QImage * ) ) );
    QObject::connect( this->m_mjpegClient, SIGNAL( gotNewImage( const QImage * ) ), this, SLOT( gotNewSensorData( const QImage * ) ) );
}

void Server::runThreadVideo()
{
    m_mjpegClient->getVideoFromStreamer( "http://localhost:8080/?action=stream" );
    m_cameraViewer->show();
}

// got new camera image
void Server::gotNewSensorData( const QImage* pic )
{

    //m_mutexCamera.lockForRead();

    static bool connected = false;

    if ( !connected )
    {
	connected = true;
	qDebug() << "OK: Camera connected";
    }

    m_camera.m_timeStamp = getCurrentTime();

    //qDebug() << "Camera time stamp: " << m_camera.m_timeStamp;

    m_camera.m_pic = *pic;

    //m_mutexCamera.unlock();
}

void Server::initThreadMotor()
{
    //m_hCOM=SetupI2CCommPort(0);
    m_hCOM = m_controller.openRS232( "/dev/ttyS0", 19200 );
    m_controller.closeRS232( m_hCOM );    
    sleep(8);
    m_hCOM = m_controller.openRS232( "/dev/ttyS0", 19200 );
    
    qDebug() << "m_hCOM: " << m_hCOM;
    if ( m_hCOM == 0 )
    {
       qDebug() << "ERROR: Robot motor not connected";

       QApplication::instance()->quit();

       return;
    }

    m_controller.SetRS232Motor33f_low_res( m_hCOM, 0x00, 0x00 );
    m_controller.SetRS232MotorPID33f( m_hCOM, 0x00, 0x00, 80, 45, 0,360);
    
    
}

void Server::runThreadMotor()
{
    QTimer *timerMotor = new QTimer( this );

    connect( timerMotor, SIGNAL( timeout() ), this, SLOT( getRobotData() ) );

    // to read robot data every 40ms (100ms ??)
    timerMotor->start( 20 );
}

void Server::getRobotData()
{
    RS232Data dataL;
    RS232Data dataR;

    char m_speed1, m_speed2;
    m_speed1 = 50;
    m_speed2 = 50;
    
    m_controller.SetRS232Motor33f_low_res( m_hCOM, m_speed1, m_speed2 );
    m_controller.GetRS232Motor33f( m_hCOM, &m_dataL, &m_dataR );

    //qDebug() << "Speed left: " << m_dataL.SpeedFront << ", Speed right: " << m_dataR.SpeedFront;
    
    emit gotNewSensorData( &dataL, &dataR );

}

// got new robot motor data
void Server::gotNewSensorData( const RS232Data* sensorL, const RS232Data* sensorR )
{
    //m_dataL = *sensorL;
    //m_dataR = *sensorR;
    
    unsigned long int timeStamp = getCurrentTime();
    
    qDebug() << "Odometry time stamp: " << timeStamp << ", Speed left: " << m_dataL.SpeedFront << ", Speed right: " << m_dataR.SpeedFront;
}

void Server::setWheelsSpeed( const char speed1, const char speed2 )
{
    //m_mutex.lock();
    int res = m_controller.SetRS232Motor33f_low_res( m_hCOM, speed1, speed2 );
    //m_mutex.unlock();
}

void Server::initThreadLaser()
{
    const char device[] = "/dev/ttyACM0";

    m_laser.NUM_SHOTS = 0;

    if ( ! m_urg.connect( device ) )
    {
	qDebug() << "ERROR: URG laser not connected (" << m_urg.what() << ")";

	QApplication::instance()->quit();

	return;
    }
    else
	qDebug() << "OK: URG laser connected";

    m_urg_params = m_urg.parameter();

    //qDebug() << m_urg_params.distance_min << ", " << m_urg_params.area_min << ", " << m_urg_params.area_max << ", " << m_urg_params.area_front << ", " << m_urg_params.area_total;

    m_laser.MAX_RANGE = m_urg_params.distance_max / 1000.0;
    m_laser.MIN_RANGE = m_urg_params.distance_min / 1000.0;
    m_laser.NUM_SHOTS = m_urg_params.area_max - m_urg_params.area_min + 1;
    m_laser.START_ANGLE = -120.0*M_PI/180.0;
    m_laser.END_ANGLE = 120.0*M_PI/180.0;
    m_laser.RESOLUTION = 360.0/m_urg_params.area_total * M_PI/180.0; //(m_laser.END_ANGLE - m_laser.START_ANGLE) / ( m_laser.NUM_SHOTS - 0 );

    m_laser.dst.resize( m_laser.NUM_SHOTS );
}

void Server::getLaserData()
{
    static vector<long> dst;
    static long timeStamp;

    int n = m_urg.capture( dst, &timeStamp );

    if (n < 0)
    {
	qDebug() << "ERROR: URG capture problem (" << m_urg.what() << ")";

	QApplication::instance()->quit();
    }
    else
    {
	emit gotNewSensorData( dst, timeStamp );
    }
}

void Server::runThreadLaser()
{
    QTimer* timerLaser = new QTimer( this );

    connect( timerLaser, SIGNAL( timeout() ), this, SLOT( getLaserData() ) );

    timerLaser->start( 100 );
}

// got new laser data
void Server::gotNewSensorData( vector<long> laserdata, const long timeStamp )
{
    m_laser.m_timeStamp = getCurrentTime();

    for ( int i = 0; i < m_laser.NUM_SHOTS; i++ )
    {
	m_laser.dst[i] = (double)laserdata[i + m_urg_params.area_min] / 1000.0;

	if ( m_laser.dst[i] <= m_laser.MIN_RANGE )
	    m_laser.dst[i] = 0;
    }

    //qDebug() << "Laser timestamp: " << m_laser.m_timeStamp << " range at 0 degree : " << m_laser.dst[m_urg_params.area_front] << " m";

    m_laserViewer->update( m_laserViewer->boundingRect() );
}

void Server::displayData()
{
    m_scene = new QGraphicsScene( QRect( 0, 0, 500, 500 ) );

    m_laserViewer = new GILaserScan( m_laser );
    m_laserViewer->setFlag( QGraphicsItem::ItemIsFocusable );

    m_scene->addItem( m_laserViewer );
    m_laserViewer->setPos( 250, 250 );

    //m_laserViewer->setFocus();
    m_scene->setFocusItem( m_laserViewer );

    m_view = new QGraphicsView();
    m_view->setScene( m_scene );
    m_view->show();

    m_view->scale( 1.0, -1.0 );


    //m_scene->addText("Hello, world!");
    //QMatrix XYFlip;
    //XYFlip.setMatrix(0.,1.,1.,0.,0.,0.);
    //m_view->setMatrix(XYFlip);

    // y is up
    //m_view->rotate(90);
    // some initial zoom
    //m_view->scale(5.0,5.0);

    //m_scene->setForegroundBrush(QBrush(Qt::lightGray, Qt::CrossPattern));

}


long Server::getCurrentTime()
{
    struct timeval time_d;

    gettimeofday( &time_d, NULL );

    return time_d.tv_sec * 1000000 + time_d.tv_usec;

}

int main( int argc, char **argv )
{
    QApplication app( argc, argv );
    
    Server server;
    
    server.initialize();
    server.run();

    return app.exec();
}
